import rcu,_thread
class rb:
    dfA=[50,90,90,90,90]
    port=[1,2,3,4,5]
    now=0
    left=11
    right=12
    either=13
    both=14
    speed=90    
    lastA=[0,0,0,0,0]    
    dfH=180
    lastH=180
    kA=[1,-1,1,1,-1]
lastTime=150
goState=0
notMove=True
def motorX(lf=0,rf=None,lb=None,rb=None,dis=None,time=None):
    if rf==None:rf=lf
    if lb==None:lb=lf
    if rb==None:rb=rf
    if dis is not None:
        for i in range(4):rcu.SetMotorCode(i+1)
        motorX(lf,rf,lb,rb)
        d1=0
        dis=dis*4
        while d1<dis:
            d1=0
            for i in range(4):d1+=abs(rcu.GetMotorCode(i+1))
        motorX(0,0,0,0)
    elif time is not None:
        motorX(lf,rf,lb,rb)
        rcu.SetWaitForTime(time)
        motorX(0,0,0,0)
    else:
        rcu.SetMotor(1,lf)
        rcu.SetMotor(2,rf)
        rcu.SetMotor(3,lb)
        rcu.SetMotor(4,rb)
def getRF():
    a = [0] * 10
    x = rcu.GetRFModuleRemoteCode(1)
    x1=(x&0xff|(x>>9&1))#将9号键的值放到x的最后一位，也就是a[0]。然后传过去，也就是发送了9,1,2,3,4,5,6,7.
    rcu.SetBluetoothData(x1)
    for i in range(1, 11):
        a[i] = (x >> i) & 1
    a[0] = x
    return a
def limit(x):
    if x>100:return 100
    if x<-100:return -100
    return int(x)
def motorMove(angle=0,v=80,k=0.88):
    angle=angle%360#将角度转换为0~359
    ds=[abs(rcu.GetMotorCode(i+1)) for i in range(4)]
    d=sum(ds)
    if(angle%90==0):
        d2=d/4
        dv=[(i-d2)*k for i in ds]
        if angle==0:motorX(limit(v-dv[0]),limit(v-dv[1]),limit(v-dv[2]),limit(v-dv[3]))
        elif angle==180:motorX(limit(-v+dv[0]),limit(-v+dv[1]),limit(-v+dv[2]),limit(-v+dv[3]))
        elif angle==90:motorX(limit(v-dv[0]),limit(-v+dv[1]),limit(-v+dv[2]),limit(v-dv[3]))
        elif angle==270:motorX(limit(-v+dv[0]),limit(v-dv[1]),limit(v-dv[2]),limit(-v+dv[3]))
def moveAngle(angle=0,dis=50,v=80,k=1.2):
    angle=angle%360
    if angle%45!=0:
        rcu.SetInBeep(1)
        rcu.SetWaitForTime(3)
        rcu.SetInBeep(0)
        return 
    for i in range(4):rcu.SetMotorCode(i+1)
    dis=dis*4
    while True:
        ds=[abs(rcu.GetMotorCode(i+1)) for i in range(4)]
        d=sum(ds)
        if d>=dis:break
        if(angle%90==0):
            d2=d/4
            dv=[(i-d2)*k for i in ds]
            if angle==0:
                motorX(limit(v-dv[0]),limit(v-dv[1]),limit(v-dv[2]),limit(v-dv[3]))
            elif angle==180:
                motorX(limit(-v+dv[0]),limit(-v+dv[1]),limit(-v+dv[2]),limit(-v+dv[3]))
            elif angle==90:
                motorX(limit(v-dv[0]),limit(-v+dv[1]),limit(-v+dv[2]),limit(v-dv[3]))
            elif angle==270:
                motorX(limit(-v+dv[0]),limit(v-dv[1]),limit(v-dv[2]),limit(-v+dv[3]))
    motorX()

def automation():
    global lastTime
    lastTime=150#倒计时清零

    moveAngle(270,150)
    flag=4#这个地方可以写成点击屏幕四个角选择程序。
    if flag==1:
        moveAngle(0,320)
        moveAngle(90,200)
        moveAngle(0,1080)
    elif flag==2:
        moveAngle(0,650)
        moveAngle(90,200)
        moveAngle(0,750)
    elif flag==3:
        moveAngle(0,970)
        moveAngle(90,200)
        moveAngle(0,430)
    else:
        moveAngle(0,1250)
        moveAngle(90,200)
        moveAngle(0,150)
    rcu.SetBluetoothData(2)
    moveAngle(0,300)
    rcu.SetBluetoothData(0)
    #前进
    # moveAngle(270,1500)
    # moveAngle(0,4000)
    # #第一次发信号
    # rcu.SetBluetoothData(0xfc)
    # rcu.SetWaitForTime(1)
    # moveAngle(0,2000)
    # #后退，前进，再次发信号
    # moveAngle(180,1500)
    # moveAngle(270,1000)
    # moveAngle(0,1500)
    # rcu.SetBluetoothData(0xfc)
    # rcu.SetWaitForTime(0.5)
    # moveAngle(270,10000)
    # motorX(-80,80)
    # rcu.SetWaitForTime(1.15)
    #舵机90度旗帜任务
    # rcu.SetMagneticServoDegreeTime(5,270,1000)
    # rcu.SetWaitForTime(4)
    # rcu.SetMagneticServoDegreeTime(5,180,1000)


    pass

def checkState(state):
    global  goState
    if goState!=state:
        goState=state
        for i in range(1,5):rcu.SetMotorCode(i)    
def main():
    global goState,notMove
    overTime=False
    autoTime=False
    while True:
        if not autoTime and rcu.GetTouchScreen():
            automation()
            autoTime=True
        a=getRF()
        if notMove and a[0]>0:
            notMove=False
            lastTime=150
        # 主控马达的运动部分
        if a[5]:            
            checkState(5)
            motorMove(0,100)
        elif a[7]:
            checkState(7)
            motorMove(180,100)
        elif a[6]:
            if a[10]:
                goState=0
                motorX(80,-80)
            else:
                checkState(6)
                motorMove(90,100)
        elif a[8]:
            if a[10]:
                goState=0
                motorX(80,-80)
            else:
                checkState(8)
                motorMove(270,100)        
        else:
            motorX(0)
           
        if a[10] and a[9]:
            if not overTime:
                motorX()
                overTime=True
                rcu.SetDisplayString(1,"凯旋",0x001F,0xF800)
                rcu.SetDisplayString(2,"而归",0x001F,0xF800)
            else:
                motorX()
                overTime=False
                rcu.SetLCDClear(0)
            rcu.SetWaitForTime(0.5)#这个时间调整到一个合适的值
        rcu.SetWaitForTime(0.01)#理论上多线程需要在循环中等待时间，哪怕很小，但是中鸣不一定按照套路出牌。所以这里需要测试，是否可以不需要这个等待时间。
def displayTime():
    global lastTime
    rcu.SetFontSize(1)
    rcu.SetDisplayString(1,"等待开始",9999,0)
    while notMove:
        rcu.SetWaitForTime(0.2)
    rcu.SetFontSize(2)
    waitTime=0.999
    while True:    
        rcu.SetInBeep(0)
        rcu.SetDisplayVar(1,lastTime,9999,0)
        rcu.SetWaitForTime(0.999)#理论上是1s但是算上其他消耗的时间，可能会超，所以这里略小于他。同时可以自己修正，和计时，估计还应该更小。
        lastTime-=1
        if lastTime<=0:lastTime=0
        if lastTime in [135,30,10,5]:#长鸣1s
            rcu.SetInBeep(1)
            rcu.SetWaitForTime(waitTime)
            rcu.SetInBeep(0)
        elif lastTime in [90,60,3,2,1]:#短鸣0.2
            rcu.SetInBeep(1)
            rcu.SetWaitForTime(0.2)
            rcu.SetInBeep(0)
            rcu.SetWaitForTime(waitTime-0.2)
        
_thread.start_new_thread(main,())
_thread.start_new_thread(displayTime,())
                